﻿#include "datatransfer.h"

DashboardData droneData;

void udpServer(std::string serverIP, int serverPort)
{
    droneData.Running = true;

    //while (droneData.Running && is_run()) {

        // 初始化Winsock库,提供UDP接口通信
        WSADATA wsaData;
        if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0) {
            std::cerr << "WSAStartup失败" << std::endl;
        }


        // 创建UDP套接字
        SOCKET sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
        if (sock == INVALID_SOCKET) {
            std::cerr << "套接字创建失败" << std::endl;
            WSACleanup();
        }

        // 设置本地地址和端口
        sockaddr_in localAddr;
        localAddr.sin_family = AF_INET;
        localAddr.sin_port = htons(8081);
        localAddr.sin_addr.s_addr = INADDR_ANY;

        // 绑定本地地址和端口
        if (bind(sock, (sockaddr*)&localAddr, sizeof(localAddr)) == SOCKET_ERROR) {
            std::cerr << "绑定失败" << std::endl;
            WSACleanup();
        }
        else {
            droneData.isConnected = true;
        }

        // 设置目标地址和端口
        sockaddr_in targetAddr;
        targetAddr.sin_family = AF_INET;
        targetAddr.sin_port = htons(serverPort);
        // 配置IP地址
        inet_pton(AF_INET, serverIP.c_str(), &(targetAddr.sin_addr));

        // 设置非阻塞模式
        // 在非阻塞模式下，recvfrom函数在没有数据时会立即返回，而不是阻塞等待
        u_long mode = 1;
        ioctlsocket(sock, FIONBIO, &mode);// 套接字设置为非阻塞模式
        // 接收数据
        constexpr int BUFFER_SIZE = 1024;  // 接收缓冲区大小
        char recvBuf[BUFFER_SIZE];         // 接收数据的缓冲区
        sockaddr_in fromAddr;              // 发送方的地址信息
        int fromLen = sizeof(fromAddr);    // 地址信息的长度
        if (keystate(key_esc)) {
            std::cout << "正在退出程序..." << std::endl;
            droneData.Running = false;
            //Sleep(1000);
        }
        // 接收数据
        // recvfrom函数返回接收到的字节数，或在出错时返回SOCKET_ERROR
        int recvSize = recvfrom(sock, recvBuf, BUFFER_SIZE, 0,
            (sockaddr*)&fromAddr, &fromLen);// 调用recvfrom接收数据

        // 处理接收到的数据
        if (recvSize > 0) {
            std::cout << "收到 " << recvSize << " 字节数据" << std::endl;
            // 检查接收的数据长度是否符合预期（4个double类型 = 32字节）
            if (recvSize == 32) {  // 4个double → 32字节
                const uint8_t* uint8Data = reinterpret_cast<const uint8_t*>(recvBuf);
                double parsedData[4];

                for (int i = 0; i < 4; ++i) {
                    uint8_t doubleBytes[8];
                    memcpy(doubleBytes, uint8Data + i * 8, 8);  // 提取8字节组

                    // （可选）若字节序不匹配，反转字节
                    // std::reverse(std::begin(doubleBytes), std::end(doubleBytes));

                    parsedData[i] = *reinterpret_cast<double*>(doubleBytes);  // 还原为double
                }
                //if (droneData.modeControl) {
                //    continue;
                //}
                // 映射到控制量（根据实际需求调整公式）
                //droneData.roll = parsedData[0] * 500 + 1500;   // 示例：0→1500，1→2000（若输入是[0,1]）
               // droneData.pitch = parsedData[1] * 500 + 1500;
                //droneData.throttle = parsedData[2] * 300 + 1000;    // 油门范围不同，单独调整
                //droneData.yaw = parsedData[3] * 500 + 1500;

                // 打印调试（验证还原是否正确）
                std::cout << "解析后数据：" << std::endl;
                for (int i = 0; i < 4; ++i) {
                    std::cout << "  原始double[" << i << "]: " << parsedData[i]
                        << " → 控制量: " << (i == 2 ? parsedData[i] * 300 + 1500 : parsedData[i] * 500 + 1500)
                        << std::endl;
                }
            }
        }
        else if (WSAGetLastError() != WSAEWOULDBLOCK) {
            // WSAEWOULDBLOCK表示无数据可接收，在非阻塞模式下属于正常情况
            std::cerr << "接收错误: " << WSAGetLastError() << std::endl;
        }
        //std::lock_guard<std::mutex> lock(droneData.mtx);
        // 发送数据
        // 构造一个包含4个double类型数据的数组
        if (droneData.modeControl)
        {
            double sendData[4] = { 0 };
            sendData[0] = droneData.roll;      //滚转
            sendData[1] = droneData.pitch;      //俯仰
            sendData[2] = droneData.throttle;     //油门
            sendData[3] = droneData.yaw;       //偏航
            // 发送数据到Simulink
            // reinterpret_cast将double数组转换为char*类型，以满足sendto函数的参数要求
            sendto(sock, reinterpret_cast<char*>(sendData), sizeof(sendData), 0,
                (sockaddr*)&targetAddr, sizeof(targetAddr));
        }
        // 清理资源
        // 关闭套接字并清理Winsock库
        SAFE_CLOSE_SOCKET(sock);
        WSACleanup();
    //}

}

void printDashboard(const DashboardData& data) {
    //std::lock_guard<std::mutex> lock(droneData.mtx);

    // 清屏并设置颜色 (Linux/macOS)
    // std::cout << "\033[2J\033[1;1H";  // 清屏
    // std::cout << "\033[1;36m";         // 青色粗体

    // 输出标题
    std::cout << "\n";
    std::cout << "╔══════════════════════════════════╗\n";
    std::cout << "║        DRONE DASHBOARD           ║\n";
    std::cout << "╠════════════════════╦═════════════╣\n";

    // 输出姿态数据
    std::cout << "║ Roll               ║ "
        << std::setw(10) << std::fixed << std::setprecision(2) << data.roll
        << "  ║\n";

    std::cout << "║ Pitch              ║ "
        << std::setw(10) << data.pitch
        << "  ║\n";

    std::cout << "║ Yaw                ║ "
        << std::setw(10) << data.yaw
        << "  ║\n";

    std::cout << "╠════════════════════╬═════════════╣\n";

    // 输出油门数据
    std::cout << "║ Throttle           ║ "
        << std::setw(10) << data.throttle
        << "  ║\n";

    std::cout << "╠════════════════════╩═════════════╣\n";

    // 输出状态信息
    std::cout << "║ UDP Status: "
        << (data.Running ? "true" : "false")
        << std::string(16, ' ') << " ║\n";

    std::cout << "╚══════════════════════════════════╝\n";
    std::cout << std::endl; // 刷新输出缓冲区
}